/*
 * ======== Standard MSP430 includes ========
 */
#include <msp430.h>

/*
 * ======== Grace related includes ========
 */
#include <ti/mcu/msp430/csl/CSL.h>
#include <stdlib.h>
#include <setjmp.h>
#include "thread.h"

#define THREADS 2

thread_t* pThreads[];
uint16_t pCurrThread;

/**
 * Starts the interrupt
 */
void ATOMIC_START(void) {
	__disable_interrupt();
}

/**
 * Stops the interrupt
 */
void ATOMIC_END(void) {
	__enable_interrupt();
}

uint16_t roundRobin() {
	return (pCurrThread + 1) % THREADS;
}

/**
 * Run the next thread
 */
void scheduler_runNextThread() {
	ATOMIC_START();
	uint16_t nextThread = roundRobin();

	switch (pThreads[nextThread]->state) {
	case THREAD_RUNNING:
		break; // already running
	case THREAD_READY:
		if (setjmp(pThreads[pCurrThread]->context) == 0) {
			// saved current execution state
			if (pThreads[pCurrThread]->state == THREAD_RUNNING)
				pThreads[pCurrThread]->state = THREAD_READY;
			pCurrThread = nextThread;
			pThreads[pCurrThread]->state = THREAD_RUNNING;
			longjmp(pThreads[pCurrThread]->context, 1);
		}
		break;
	default:
		break; // no thread to run -> sleep no atomic
	}
	ATOMIC_END();
}

/**
 * Handle interrupt
 */
void schedulerInterrupt(void) {
 scheduler_runNextThread();
}


void init(void) {
	ATOMIC_START();
	WDTCTL = WDTPW + WDTHOLD; //Stop watchdog
	*pThreads = malloc(sizeof(thread_t*) * THREADS);
	pThreads[0] = createThread1(1);
	pThreads[1] = createThread2(2);
	pCurrThread = pThreads[0]->id;
	ATOMIC_END();
}

/*
 *  ======== main ========
 */
int main(int argc, char *argv[]) {
	CSL_init(); // Activate Grace-generated configuration

	init();

	while (1) {

	}

}
